#include "foc.h"

void inverse_clarke(int32_t alpha, int32_t beta, int32_t *a, int32_t *b, int32_t *c)
{
  *a = alpha;
  *b = (-8192 * alpha + 14189 * beta) >> 14; // 0.8660254 = sqrt(3)/2
  *c = (-8192 * alpha - 14189 * beta) >> 14;
}

void inverse_park(int32_t d, int32_t q, float theta, int32_t *alpha, int32_t *beta)
{
  int32_t sin_val = sinf(theta) * 32768;
  int32_t cos_val = cosf(theta) * 32768;

  *alpha = d * cos_val - q * sin_val;
  *beta  = d * sin_val + q * cos_val;
	*alpha >>= 15;
	*beta >>= 15;
}

void machinery_angle_to_electric_angle(foc_t *foc,float angle)
{
	foc->angle = angle * foc->k + foc->b;
}

void foc_init(foc_t *foc,float *angle)
{
	//校准
	
}

int32_t zero_processing(int32_t target,int32_t fb,int32_t midfb)
{
	int32_t e = target - fb;
	if(e > midfb)
	{
		return fb + (midfb << 1);
	}
	else if(e < -midfb)
	{
		return fb - (midfb << 1);
	}
	return fb;
}
